#include <ace/config.h>
#include <iostream>
#include <vector>
#include <string>
#include <algorithm>
#include <sstream>
#include <fstream>
#include <sys/stat.h>

#include <yarp/sig/all.h>
#include <yarp/os/all.h>
#include <yarp/dev/all.h>

#include <cv.h>
#include <cvaux.h>
#include <highgui.h>

#define MAX_COUNT 100
#define WIN_SIZE 10
#define FLOW_RATIO 80
#define FEAT_QLTY 0.01
#define FEAT_MIN_DIST 5.0
#define TRESHOLD 40
#define NUM_X 9
#define NUM_Y 6
#define WAIT_TIME 100
#define DEF 1
#define PI 3.14159265

using namespace std;

using namespace yarp::dev;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::sig::draw;
using namespace yarp::sig::file;

bool fileExists(string strFilename);

int main(int argc, char *argv[])
{  
  ImageOf<PixelRgb> tempYImage;
  IplImage *tempCImage;    
  IplImage *leftImg, *rightImg, *leftGray, *rightGray;
  IplImage *gray;
  IplImage *result3DImage, *imageX, *imageY, *imageZ;
  IplImage *result2DPoints;
  int displayCorners = 0;
  int showUndistorted = 1;
  const int maxScale = 1;
  const float squareSize = 48.0;
  int n, N;
  int count;
  int result;
  int i, j, k;
  int nframes;
  CvSize imageSize = {0, 0};
  
  vector<CvPoint3D32f> objectPoints;
  vector<CvPoint2D32f> points[2];
  vector<int> npoints;
  vector<uchar> active[2];
  vector<CvPoint2D32f> temp;

  bool flag;
  long begin;
  istringstream dataElement;
  
  CvStereoBMState *bmState = cvCreateStereoBMState();
  
  double m1[3][3], m2[3][3], d1[5], d2[5], rot[3][3], trans[3], ess[3][3], fund[3][3];
  double rotX[3][3], rotY[3][3], rotZ[3][3];
  double r1[3][3], r2[3][3], p1[3][4], p2[3][4], q[4][4];
  double d1Zeros[5], d2Zeros[5];
  CvMat _m1 = cvMat(3, 3, CV_64F, m1);
  CvMat _m2 = cvMat(3, 3, CV_64F, m2);
  CvMat _d1 = cvMat(1, 5, CV_64F, d1);
  CvMat _d2 = cvMat(1, 5, CV_64F, d2);
  CvMat _rot = cvMat(3, 3, CV_64F, rot);
  CvMat _rotX = cvMat(3, 3, CV_64F, rotX);
  CvMat _rotY = cvMat(3, 3, CV_64F, rotY);
  CvMat _rotZ = cvMat(3, 3, CV_64F, rotZ);
  CvMat _trans = cvMat(3, 1, CV_64F, trans);
  CvMat _ess = cvMat(3, 3, CV_64F, ess);
  CvMat _fund = cvMat(3, 3, CV_64F, fund);
  CvMat _r1 = cvMat(3, 3, CV_64F, r1);
  CvMat _r2 = cvMat(3, 3, CV_64F, r2);
  CvMat _p1 = cvMat(3, 4, CV_64F, p1);
  CvMat _p2 = cvMat(3, 4, CV_64F, p2);

  CvMat _q = cvMat(4, 4, CV_64F, q);
  CvMat _d1Zeros = cvMat(1, 5, CV_64F, d1Zeros);
  CvMat _d2Zeros = cvMat(1, 5, CV_64F, d2Zeros);
  
  
  CvMat part;
  
  ofstream disparityFile;
  
  n = NUM_X * NUM_Y;
  N = 0;
  temp.resize(n, cvPoint2D32f(0, 0));
  
  cvNamedWindow("OriginalLeft", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("GrayLeft", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("OriginalRight", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("GrayRight", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("Disparity", CV_WINDOW_AUTOSIZE);
  
  yarp::sig::file::read(tempYImage, "/home/nigno/Immagini/dataset/calibrazioneicub/Left0.ppm");
      
  // Convert the image in a openCv image
  tempCImage = cvCreateImage(cvSize(tempYImage.width(),  
			     tempYImage.height()), 
			     IPL_DEPTH_8U, 3 );
  
  cvCvtColor((IplImage*)tempYImage.getIplImage(), tempCImage, CV_RGB2BGR);
  
  gray = cvCreateImage(cvGetSize(tempCImage), 8, 1);
  
  imageSize = cvGetSize(tempCImage);
  
  cvCvtColor(tempCImage, gray, CV_BGR2GRAY);
  
  CvMat *mx1 = cvCreateMat(imageSize.height, imageSize.width, CV_32F);
  CvMat *my1 = cvCreateMat(imageSize.height, imageSize.width, CV_32F);
  CvMat *mx2 = cvCreateMat(imageSize.height, imageSize.width, CV_32F);
  CvMat *my2 = cvCreateMat(imageSize.height, imageSize.width, CV_32F);
  
  IplImage *mapXDis = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  IplImage *mapYDis = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  IplImage *mapXDis2 = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  IplImage *mapYDis2 = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  IplImage *tempDistImage;
  
  cvSetIdentity(&_m1);
  cvSetIdentity(&_m2);
  cvZero(&_d1);
  cvZero(&_d2);

//  old icub
  
//  m1[0][0] = 258.245;
//  m1[1][1] = 257.123;
//  m1[0][2] = 157.631;
//  m1[1][2] = 113.148;
//  m2[0][0] = 257.2;
//  m2[1][1] = 257.436;
//  m2[0][2] = 155.086;
//  m2[1][2] = 130.77;
  
//  d1[0] = -0.425346;
//  d1[1] = 0.146858;
//  d1[2] = -0.00271551;
//  d1[3] = 0.00490219;
//  d1[4] = 0;
//  d2[0] = -0.41324;
//  d2[1] = 0.158982;
//  d2[2] = -0.00148367;
//  d2[3] = 0.000609254;
//  d2[4] = 0;

// nao

  m1[0][0] = 251.103;
  m1[1][1] = 248.425;
  m1[0][2] = 136.058;
  m1[1][2] = 115.946;
  m2[0][0] = 257.023;
  m2[1][1] = 254.647;
  m2[0][2] = 177.269;
  m2[1][2] = 125.096;

  d1[0] = -0.457156;
  d1[1] = 0.303359;
  d1[2] = -0.00273465;
  d1[3] = 0.00188351;
  d1[4] = -0.13247;
  d2[0] = -0.446377;
  d2[1] = 0.247249;
  d2[2] = 0.000347644;
  d2[3] = -0.000379765;
  d2[4] = -0.0884467;

  
//  cvInitUndistortMap(&_m1, &_d1, mapXDis, mapYDis);
//  cvInitUndistortMap(&_m2, &_d2, mapXDis2, mapYDis2);
  
  //left eye chessboard extraction
  i = -1;
  do
  { 
    result = 0;
    count = 0;
    i++;
    cout << i << " Lettura immagine sinistra -";
    // Read a yarp image from file
    begin = dataElement.tellg();
    ostringstream iStrStream ;
    iStrStream << "/home/nigno/Immagini/dataset/calibrazionenao2/Left" << i << ".ppm";
//    iStrStream << "/home/nigno/Immagini/dataset/naoSinistra/Left" << i << ".ppm";
    cout << " Controllo esistenza file -";
    flag = fileExists(iStrStream.str().c_str());
    iStrStream.seekp(begin);
    
    if(flag == true)
    {
      cout << " Lettura file" << endl;
      yarp::sig::file::read(tempYImage, iStrStream.str().c_str());
      
      // Convert the image in a openCv image
      tempCImage = cvCreateImage(cvSize(tempYImage.width(),  
                                        tempYImage.height()),
                                        IPL_DEPTH_8U, 3 );
  
      cvCvtColor((IplImage*)tempYImage.getIplImage(), tempCImage, CV_RGB2BGR);
      
//      tempDistImage = cvCloneImage(tempCImage);
//      cvRemap(tempDistImage, tempCImage, mapXDis, mapYDis);
      
      cvCvtColor(tempCImage, gray, CV_BGR2GRAY);
 
      //find the chessboard inside hte image
      result = cvFindChessboardCorners(tempCImage, cvSize(NUM_X, NUM_Y), &temp[0], &count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
      
      //draw the chessboard points on the image
      cvDrawChessboardCorners(tempCImage, cvSize(NUM_X, NUM_Y), &temp[0], count, result);
      
      //find the subpix values
      N = points[0].size();

      points[0].resize(N + n, cvPoint2D32f(0, 0));
      active[0].push_back((uchar) result);
      
      if(result)
      {
	cvFindCornerSubPix(gray, &temp[0], count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 30, 0.01));
	copy(temp.begin(), temp.end(), points[0].begin() + N);
      }
      
      // Show the openCv image
      cvShowImage("OriginalLeft", tempCImage);
      cvShowImage("GrayLeft", gray);
	
      // Wait
      cvWaitKey(WAIT_TIME);
    }
  }
  while(flag == true);
  cout << "Fine" << endl;
  
  //right eye chessboard extraction
  i = -1;
  do
  { 
    result = 0;
    count = 0;
    i++;
    cout << i << " Lettura immagine destra -";
    // Read a yarp image from file
    begin = dataElement.tellg();
    ostringstream iStrStream ;
    iStrStream << "/home/nigno/Immagini/dataset/calibrazionenao2/Right" << i << ".ppm";
//    iStrStream << "/home/nigno/Immagini/dataset/naoDestra/Right" << i << ".ppm";
    cout << " Controllo esistenza file -";
    flag = fileExists(iStrStream.str().c_str());
    iStrStream.seekp(begin);
      
    if(flag == true)
    {
      cout << " Lettura file" << endl;
      yarp::sig::file::read(tempYImage, iStrStream.str().c_str());
      
      // Convert the image in a openCv image
      tempCImage = cvCreateImage(cvSize(tempYImage.width(),  
					 tempYImage.height()), 
					 IPL_DEPTH_8U, 3 );
  
      cvCvtColor((IplImage*)tempYImage.getIplImage(), tempCImage, CV_RGB2BGR);
      
      // Undistortion
  
//      tempDistImage = cvCloneImage(tempCImage);
//      cvRemap(tempDistImage, tempCImage, mapXDis2, mapYDis2);

      cvCvtColor(tempCImage, gray, CV_BGR2GRAY);
      
      //find sthe chessboard inside hte image
      result = cvFindChessboardCorners(tempCImage, cvSize(NUM_X, NUM_Y), &temp[0], &count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
      
      //draw the chessboard points on the image
      cvDrawChessboardCorners(tempCImage, cvSize(NUM_X, NUM_Y), &temp[0], count, result);
      
      //find the subpix values
      N = points[1].size();
      points[1].resize(N + n, cvPoint2D32f(0, 0));
      active[1].push_back((uchar) result);
      
      if(result)
      {
	cvFindCornerSubPix(gray, &temp[0], count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 30, 0.01));
	copy(temp.begin(), temp.end(), points[1].begin() + N);
      }
      
      // Show the openCv image
      cvShowImage("OriginalRight", tempCImage);
      cvShowImage("GrayRight", gray);
	
      // Wait
      cvWaitKey(WAIT_TIME);
    }
  }
  while(flag == true);
  
  cout << "Fine" << endl;
  
  cout << "----------Calibrating the stereo cameras----------" << endl;
  
  //calculate the object relative chessboard's points position
  nframes = active[0].size();
  objectPoints.resize(nframes * n);
  
  for(i = 0; i < NUM_Y; i++)
    for(j = 0; j < NUM_X; j++)
      objectPoints[(i * NUM_X) + j] = cvPoint3D32f(i * squareSize, j * squareSize, 0);

  for(i = 0; i < nframes; i++)
    copy(objectPoints.begin(), objectPoints.begin() + n, objectPoints.begin() + (i * n));
  
  npoints.resize(nframes, n);
  N = n * nframes;
  
  //initialise matrices
  CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0]);
  CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0]);
  CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0]);
  CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0]);
  
  // calibrate the stereo cameras-5.09035
  
  for(i=0; i < 5; i++)
  {
//    d1Zeros[i] = 0.00;
//    d2Zeros[i] = 0.00;
     d1Zeros[i] = d1[i];
     d2Zeros[i] = d2[i];
  }

  // calibrate mono

//  cvCalibrateCamera2(&_objectPoints, &_imagePoints1, &_npoints, imageSize, &_m1, &_d1Zeros, NULL, NULL, 0);
//  cvCalibrateCamera2(&_objectPoints, &_imagePoints2, &_npoints, imageSize, &_m2, &_d2Zeros, NULL, NULL, 0);

  cvStereoCalibrate(&_objectPoints, &_imagePoints1, &_imagePoints2, &_npoints, 
		    &_m1, &_d1Zeros, &_m2, &_d2Zeros, 
		    imageSize, 
		    &_rot, &_trans, &_ess, &_fund,
                    cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5),
                    /*CV_CALIB_FIX_ASPECT_RATIO + */CV_CALIB_FIX_INTRINSIC +
                    CV_CALIB_FIX_K1 + CV_CALIB_FIX_K2 + CV_CALIB_FIX_K3 +
                    CV_CALIB_ZERO_TANGENT_DIST/* + CV_CALIB_SAME_FOCAL_LENGTH*/);
  
  // if you want to check the result write here the checking code

  for(i=0; i < 5; i++)
  {
    d1[i] = 0.00;
    d2[i] = 0.00;
  }


  cvInitUndistortMap(&_m1, &_d1Zeros, mapXDis, mapYDis);
  cvInitUndistortMap(&_m2, &_d2Zeros, mapXDis2, mapYDis2);
  
  // calculate retification parameter
  cout << "Calculating retification parameters..." << endl; 
  
  cvStereoRectify(&_m1, &_m2, &_d1, &_d2,
		  imageSize, 
		  &_rot, &_trans,
		  &_r1, &_r2, &_p1, &_p2, &_q,
		  0);
  
  cvInitUndistortRectifyMap(&_m1, &_d1, &_r1, &_p1, mx1, my1);
  cvInitUndistortRectifyMap(&_m2, &_d2, &_r2, &_p2, mx2, my2);

  // load stereo images
  cout << "Loading stereo images" << endl;
  
  // left
  yarp::sig::file::read(tempYImage, "/home/nigno/Immagini/dataset/Left0.ppm");
      
  // Convert the image in a openCv image
  leftImg = cvCreateImage(cvSize(tempYImage.width(),  
				 tempYImage.height()), 
                                 IPL_DEPTH_8U, 3 );
  
  cvCvtColor((IplImage*)tempYImage.getIplImage(), leftImg, CV_RGB2BGR);
  
//   cvSaveImage("../../../3dVisionDatas/maialinaLeft.jpg", leftImg);
  
  imageSize = cvGetSize(leftImg);
  
  leftGray = cvCreateImage(cvGetSize(leftImg), 8, 1);
  cvCvtColor(leftImg, leftGray, CV_BGR2GRAY);
  
  // right
  yarp::sig::file::read(tempYImage, "/home/nigno/Immagini/dataset/Right0.ppm");
  
  // Convert the image in a openCv image
  rightImg = cvCreateImage(cvSize(tempYImage.width(),  
				 tempYImage.height()), 
				 IPL_DEPTH_8U, 3 );
  
  cvCvtColor((IplImage*)tempYImage.getIplImage(), rightImg, CV_RGB2BGR);

  rightGray = cvCreateImage(cvGetSize(rightImg), 8, 1);
  
  cvCvtColor(rightImg, rightGray, CV_BGR2GRAY);
  
//  // Undistortion
  
//  // invert
//   for(i = 0; i < 5; i++)
//   {
//     d1Zeros[i] = -d1Zeros[i];
//     d2Zeros[i] = -d2Zeros[i];
//   }
  
   tempDistImage = cvCloneImage(leftGray);
   cvRemap(tempDistImage, leftGray, mapXDis, mapYDis);
   tempDistImage = cvCloneImage(rightGray);
   cvRemap(tempDistImage, rightGray, mapXDis2, mapYDis2);

//  // end undistortion
  
  CvMat *img1r = cvCreateMat(imageSize.height, imageSize.width, CV_8U);
  CvMat *img2r = cvCreateMat(imageSize.height, imageSize.width, CV_8U);
  
//   double dispDoub[imageSize.height][imageSize.width];
//   CvMat _disp = cvMat(imageSize.height, imageSize.width, CV_16S, dispDoub);
//   cvZero(&_disp);
// 
//   CvMat *disp = &_disp;
  CvMat *disp = cvCreateMat(imageSize.height, imageSize.width, /*CV_16S*/ CV_32F);
  CvMat *vdisp = cvCreateMat(imageSize.height, imageSize.width, CV_8U);
  CvMat *pair = cvCreateMat(imageSize.height, imageSize.width * 2, CV_8UC3);
  
  result3DImage = cvCreateImage(cvGetSize(leftImg), IPL_DEPTH_32F, 3);
  result2DPoints = cvCreateImage(cvGetSize(leftImg), IPL_DEPTH_32F, 3);

  // set correspondence parameters
  assert(bmState != 0);
  if (DEF == 2)
  {
    bmState->preFilterSize = 41;
    bmState->preFilterCap = 31;
    bmState->SADWindowSize = 41;
    bmState->minDisparity = -64;
    bmState->numberOfDisparities = 128;
    bmState->textureThreshold = 10;
    bmState->uniquenessRatio = 15;
  }
  else if (DEF == 1)
  {
      int nStereoPreFilterType = CV_STEREO_BM_NORMALIZED_RESPONSE;
      int nStereoPreFilterSize= 20;//33;
      int nStereoPreFilterCap=19;
      int nStereoSADWindowSize=20;//31;//22;
      int nStereoMinDisparity =4;//6; //this is multiplied by -16
      int nStereoNumDisparities =4;//10;//14; //this is multiplied by 16
      int nStereoTextureThreshold =12;
      int nStereoUniquenessRatio =3;//9;
      int nStereoFillUnmatchedRegions = 1;
      int nStereoSpeckleWindowSize = 0;
      int nStereoSpeckleRange = 0;

      if( nStereoPreFilterSize % 2 == 0 ) nStereoPreFilterSize++;
      if( nStereoPreFilterSize < 5 ) nStereoPreFilterSize = 5;
      if( nStereoPreFilterSize > 255) nStereoPreFilterSize = 255;
      if( nStereoPreFilterCap < 1 ) nStereoPreFilterCap = 1;
      if( nStereoPreFilterCap > 63 ) nStereoPreFilterCap = 63;
      if( nStereoSADWindowSize % 2 == 0 ) nStereoSADWindowSize++;
      if( nStereoSADWindowSize < 5 ) nStereoSADWindowSize = 5;
      if( nStereoSADWindowSize > 255 )nStereoSADWindowSize = 255;
      if( nStereoSADWindowSize >= MIN(imageSize.width, imageSize.height) )
      nStereoSADWindowSize = MIN(imageSize.width, imageSize.height);
      if( nStereoMinDisparity < 0) nStereoMinDisparity=0;
      if( nStereoNumDisparities <1 ) nStereoNumDisparities=1;
      if( nStereoTextureThreshold < 0 ) nStereoTextureThreshold = 1;
      if( nStereoUniquenessRatio < 0 ) nStereoUniquenessRatio = 1;

      bmState->speckleWindowSize = nStereoSpeckleWindowSize;
      bmState->speckleRange = nStereoSpeckleRange;
      bmState->preFilterSize=nStereoPreFilterSize; // 41
      bmState->preFilterCap=nStereoPreFilterCap; //31
      bmState->SADWindowSize=nStereoSADWindowSize; //41
      bmState->minDisparity=nStereoMinDisparity*-16;//-64
      bmState->numberOfDisparities=nStereoNumDisparities*16; //128
      bmState->textureThreshold=nStereoTextureThreshold; //10
      bmState->uniquenessRatio=nStereoUniquenessRatio; //15
  }
  else
  {
    int numberOfDisparities = imageSize.width / 8;
    numberOfDisparities -= numberOfDisparities % 16;
    
    //bmState->roi1 = roi1;
    //bmState->roi2 = roi2;
    bmState->preFilterCap = 31;
    bmState->SADWindowSize = 15;
    bmState->minDisparity = 0;
    bmState->numberOfDisparities = numberOfDisparities;
    bmState->textureThreshold = 10;
    bmState->uniquenessRatio = 15;
    bmState->speckleWindowSize = 100;
    bmState->speckleRange = 32;
    bmState->disp12MaxDiff = 1;
  }
  
  // rectify images
  cvRemap(leftGray, img1r, mx1, my1);
  cvRemap(rightGray, img2r, mx2, my2);
 
  // find correspondence
  cvFindStereoCorrespondenceBM(img1r, img2r, disp, bmState);
  cvNormalize(disp, vdisp, 0, 256, CV_MINMAX);
  
// posizione 3D a manina.  
  
  int rows, columns;
  rows = 4;
  columns = imageSize.height * imageSize.width;
  
  double pos2D[rows][columns], pos3D[rows][columns];
  CvMat _pos2D = cvMat(rows, columns, CV_64F, pos2D);
  CvMat _pos3D = cvMat(rows, columns, CV_64F, pos3D);
  cvZero(&_pos2D);
  cvZero(&_pos3D);
  
  for(i = 0; i < imageSize.height; i++)
    for(j = 0; j < imageSize.width; j++)
    {
      const float *ptr = (const float*)(disp->data.ptr + i * disp->step + j);
      
      pos2D[0][(i * imageSize.width) + j] = j;
      pos2D[1][(i * imageSize.width) + j] = i;
      pos2D[2][(i * imageSize.width) + j] = CV_MAT_ELEM(*disp, double, i, j);
      pos2D[3][(i * imageSize.width) + j] = 1;
    }
  
  cvMatMul(&_q, &_pos2D, &_pos3D);
    
    
  cvReprojectImageTo3D(disp, result3DImage, &_q);
  
  int height     = result3DImage->height;
  int width      = result3DImage->width;
  int step       = result3DImage->widthStep/sizeof(float);
  int channels   = result3DImage->nChannels;
  float *data    = (float *)result3DImage->imageData;
  float min, max, max2;
  float w;
  int countNumPoints;
  countNumPoints = 0;
  min = 1000000;
  max = -1000000;
  max2 = -1000000;
  for(i = 0; i < height; i++)
    for(j = 0; j < width; j++)
    {
      if(data[i * step + j * channels + 2] < min)
	min = data[i * step + j * channels + 2];
      if(data[i * step + j * channels + 2] > max)
	max = data[i * step + j * channels + 2];
    }
  for(i = 0; i < height; i++)
    for(j = 0; j < width; j++)
      if(data[i * step + j * channels + 2] > max2 && data[i * step + j * channels + 2] < max)
      {
	max2 = data[i * step + j * channels + 2];
      }
  for(i = 0; i < height; i++)
    for(j = 0; j < width; j++)
    {
      if(data[i * step + j * channels + 2] > max2)
      {
	data[i * step + j * channels + 0] = 0;
	data[i * step + j * channels + 1] = 0;
	data[i * step + j * channels + 2] = max2;
	countNumPoints++;
      }
    }
    
    // Draw a line to test the retification
  for(i = 0; i < 240; i += 10)
  {
    cvLine(img1r, cvPoint(0,i), cvPoint(319, i), cvScalar(255, 0, 0), 0, 0);
    cvLine(img2r, cvPoint(0,i), cvPoint(319, i), cvScalar(255, 0, 0), 0, 0);
  }  
  
  for(i = 0; i < 240; i += 20)
  {
    cvLine(leftGray, cvPoint(0,i), cvPoint(319, i), cvScalar(255, 0, 0), 0, 0);
    cvLine(rightGray, cvPoint(0,i), cvPoint(319, i), cvScalar(255, 0, 0), 0, 0);
  }
  
  for(i = 0; i < 360; i += 20)
  {
    cvLine(leftGray, cvPoint(i,0), cvPoint(i, 239), cvScalar(255, 0, 0), 0, 0);
    cvLine(rightGray, cvPoint(i,0), cvPoint(i, 239), cvScalar(255, 0, 0), 0, 0);
  } 
    
  // Show the openCv images
  cvShowImage("OriginalLeft", leftGray);
  cvShowImage("GrayLeft", img1r);
  cvShowImage("OriginalRight", rightGray);
  cvShowImage("GrayRight", img2r);  
  
  cvShowImage("Disparity", vdisp);
  
  //-------------start print----------------------------
  
  cout << "M1" << endl;
  for(i = 0; i < 3; i++)
  {
    for(j = 0; j < 3; j++)
    {
      cout << m1[i][j] << " ";
    }
    cout << endl;
  }
  
  cout << "M2" << endl;
  for(i = 0; i < 3; i++)
  {
    for(j = 0; j < 3; j++)
    {
      cout << m2[i][j] << " ";
    }
    cout << endl;
  }

  cout << "D1" << endl;
  for(j = 0; j < 5; j++)
  {
    cout << d1Zeros[j] << " ";
  }
  cout << endl;
  
  cout << "D2" << endl;
  for(j = 0; j < 5; j++)
  {
    cout << d2Zeros[j] << " ";
  }
  cout << endl;
  
  cout << "R" << endl;
  for(i = 0; i < 3; i++)
  {
    for(j = 0; j < 3; j++)
    {
      cout << rot[i][j] << " ";
    }
    cout << endl;
  }
  
  cout << "T" << endl;
  for(j = 0; j < 3; j++)
  {
    cout << trans[j] << " ";
  }
  cout << endl;
  
  cout << "Q" << endl;
  for(i = 0; i < 4; i++)
  {
    for(j = 0; j < 4; j++)
    {
      cout << q[i][j] << " ";
    }
    cout << endl;
  }
  //-------------end print----------------------------
  
  int key;
  
  while (key != 'q')
    key = cvWaitKey(100);
  
  cvShowImage("Disparity", result3DImage);
  
  while (key != 't')
    key = cvWaitKey(100);
  
  double object_points[countNumPoints][3];
  CvMat _object_points = cvMat(countNumPoints, 3, CV_64F, object_points);
  double image_points[countNumPoints][2];
  CvMat _image_points = cvMat(countNumPoints, 2, CV_64F, image_points);
  
  double obRot[3];
  CvMat _obRot = cvMat(3, 1, CV_64F, obRot);
  double obTran[3];
  CvMat _obTran = cvMat(3, 1, CV_64F, obTran);
  
  for(i = 0; i < 3; i++)
  {
    obRot[i] = 0;
    obTran[i] = 0;
  }
  
  double qInv[4][4];
  CvMat _qInv = cvMat(4, 4, CV_64F, qInv);

  cvInvert(&_q, &_qInv);
  
  float angle = 0;
  
  rotX[0][0] = 1.0;
  rotX[0][1] = 0.0;
  rotX[0][2] = 0.0;
  rotX[1][0] = 0.0;
  rotX[1][1] = cos(angle*PI/180);
  rotX[1][2] = sin(angle*PI/180);
  rotX[2][0] = 0.0;
  rotX[2][1] = -sin(angle*PI/180);
  rotX[2][2] = cos(angle*PI/180);
  
//   angle = -7.1;
  angle = 0;

  rotY[0][0] = cos(angle*PI/180);
  rotY[0][1] = 0.0;
  rotY[0][2] = -sin(angle*PI/180);
  rotY[1][0] = 0.0;
  rotY[1][1] = 1.0;
  rotY[1][2] = 0.0;
  rotY[2][0] = sin(angle*PI/180);
  rotY[2][1] = 0.0;
  rotY[2][2] = cos(angle*PI/180);
  
//   angle = 0;
  angle = 0;
  
  rotZ[0][0] = cos(angle*PI/180);
  rotZ[0][1] = sin(angle*PI/180);
  rotZ[0][2] = 0.0;
  rotZ[1][0] = -sin(angle*PI/180);
  rotZ[1][1] = cos(angle*PI/180);
  rotZ[1][2] = 0.0;
  rotZ[2][0] = 0.0;
  rotZ[2][1] = 0.0;
  rotZ[2][2] = 1.0;
  
  CvMat *rotzy = cvCreateMat( 3, 3, CV_64FC1);
  
  cvMatMul(&_rotZ, &_rotY, rotzy);
  cvMatMul(rotzy, &_rotX, &_rot);
  
  float rotx, roty, rotz;
  int newx, newy;
  
  IplImage *rotatedImage = cvCreateImage(imageSize, 8, 1);  
  float *data2 = (float *)rotatedImage->imageData;
  float *data3 = (float *)result2DPoints->imageData;
  int step2       = rotatedImage->widthStep/sizeof(float);
  int channels2   = rotatedImage->nChannels;
  int step3       = result2DPoints->widthStep/sizeof(float);
  int channels3   = result2DPoints->nChannels;
  int countNumPoints2;
  countNumPoints2 = 0;

  for(i = 0; i < height; i++)
    for(j = 0; j < width; j++)
    {
      if (data[i * step + j * channels + 0] != 0 || data[i * step + j * channels + 1] != 0)
      {
	rotx = (rot[0][0] * data[i * step + j * channels + 0]) +
	       (rot[0][1] * data[i * step + j * channels + 1]) +
	       (rot[0][2] * data[i * step + j * channels + 2]);
	roty = (rot[1][0] * data[i * step + j * channels + 0]) +
	       (rot[1][1] * data[i * step + j * channels + 1]) +
	       (rot[1][2] * data[i * step + j * channels + 2]);
	rotz = (rot[2][0] * data[i * step + j * channels + 0]) +
	       (rot[2][1] * data[i * step + j * channels + 1]) +
	       (rot[2][2] * data[i * step + j * channels + 2]);
	newx = (qInv[0][0] * rotx) +
	       (qInv[0][1] * roty) +
	       (qInv[0][2] * rotz) +
	       qInv[0][3];
	newy = (qInv[1][0] * rotx) +
	       (qInv[1][1] * roty) +
	       (qInv[1][2] * rotz) +
	       qInv[1][3];
	       
	if(newy >= 0 && newy < height && newx >= 0 && newx < width)
	  data2[newy * step2 + newx * channels2 + 0] = CV_MAT_ELEM(*img1r, double, i, j);
      }
    }
  
  cvSave("result3dImage.txt", result3DImage);
  
  // show the disparity map
  cvShowImage("Disparity", rotatedImage);
  
  // wait
  
  while (key != 'q')
    key = cvWaitKey(100);
  
  cvDestroyWindow("OriginalLeft");
  cvDestroyWindow("GrayLeft");
  cvDestroyWindow("OriginalRight");
  cvDestroyWindow("GrayRight");
  cvDestroyWindow("Disparity");
  
  cvReleaseImage(&tempCImage);
  cvReleaseImage(&gray);
  cvReleaseImage(&leftImg);
  cvReleaseImage(&rightImg);
  cvReleaseImage(&leftGray);
  cvReleaseImage(&rightGray);
  cvReleaseImage(&imageX);
  cvReleaseImage(&imageY);
  cvReleaseImage(&imageZ);
  cvReleaseImage(&rotatedImage);
  cvReleaseImage(&result3DImage);
  cvReleaseImage(&result2DPoints);
  cvReleaseImage(&mapXDis);
  cvReleaseImage(&mapXDis2);
  cvReleaseImage(&mapYDis);
  cvReleaseImage(&mapYDis2);
  cvReleaseImage(&tempDistImage);
  
  
  cvReleaseStereoBMState(&bmState);
  
  cvReleaseMat(&mx1);
  cvReleaseMat(&my1);
  cvReleaseMat(&mx2);
  cvReleaseMat(&my2);
  cvReleaseMat(&img1r);
  cvReleaseMat(&img2r);
  cvReleaseMat(&disp);
  cvReleaseMat(&vdisp);
  cvReleaseMat(&pair);
  cvReleaseMat(&rotzy);
  
  return 0;
}

bool fileExists(string strFilename) { 
  struct stat stFileInfo; 
  bool blnReturn; 
  int intStat; 

  intStat = stat(strFilename.c_str(),&stFileInfo); 
 
  if(intStat == 0) 
  { 
    blnReturn = true; 
  } 
  else 
  { 
    blnReturn = false; 
  } 
   
  return(blnReturn); 
}



